
#include "PgrUsbCam.h"

PgrUsbCam::
PgrUsbCam(PGRGuid* guid, const string& cam_name,
	  ros::NodeHandle nh, image_transport::ImageTransport& it)
  :name_(cam_name),info_mgr_(ros::NodeHandle(nh, name_), name_),
   pub_(it.advertiseCamera(name_+"/image_raw", QUEUE_SIZE))
{
  Error error;
  error = cam_.Connect(guid);
  if (error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());
  else
    {
      connected_flag = true;
      ROS_INFO("Camera connected: %s", cam_name.c_str());
    }
}

PgrUsbCam::~PgrUsbCam()
{
  if (running_flag)
    cam_.StopCapture();
  
  if (connected_flag)
    cam_.Disconnect();
}

void
PgrUsbCam::configCamParams(int exp, int brt, double gain, double shtr)
{
  configVideoFormat();
  configImageProperty();

    /* 
     * If Shutter and Gain are both set to auto, then auto exposure
     * will tune each one to maintain a constant exposure at each pixel. 
     *
     * If only one of Shutter/Gain is set to auto, then that value
     * will be tuned to enforce the constant exposure value at each
     * pixel, while the other value is not changed.
     *
     * If both Shutter and Gain are set to manual, then auto exposure
     * has no effect.
     *
     */
  configTriggerMode(false);
//  setBrightness(false, brt);
//  setExposure(false, true, exp);
//  setGain(false, gain);
  setShutter(false, shtr);

//  cam_.StartCapture();
//  ROS_INFO("Cam %s tuning...", name_.c_str());
//  sleep(3);// wait for the settings to stablize
//  cam_.StopCapture();
  configTriggerMode(true);
}

void
PgrUsbCam::configCamInfo(const string& frameID, const string& info_url)
{
  
  // Verify the url first
  if(info_mgr_.validateURL(info_url))
    {
      // try loading the CameraInfo.
      // It should fail if the camera is not calibrated
      if(info_mgr_.loadCameraInfo(info_url))
	info_ = info_mgr_.getCameraInfo();
    }
  
  info_.header.frame_id = frameID;
}

bool
PgrUsbCam::configVideoFormat()
{
  Format7ImageSettings fmt7ImageSettings;
  fmt7ImageSettings.mode        = MODE_0;
  fmt7ImageSettings.pixelFormat = PIXEL_FORMAT_RAW8;
  fmt7ImageSettings.offsetX     = 0;
  fmt7ImageSettings.offsetY     = 0;
  fmt7ImageSettings.width       = MAX_IMAGE_WIDTH;
  fmt7ImageSettings.height      = MAX_IMAGE_HEIGHT;

  bool valid;
  Format7PacketInfo fmt7PacketInfo;
  Error error;
  error = cam_.ValidateFormat7Settings(&fmt7ImageSettings,
				       &valid,
				       &fmt7PacketInfo);

  if( error != PGRERROR_OK)
    { ROS_ERROR(error.GetDescription()); return false; }

  error = cam_.SetFormat7Configuration(&fmt7ImageSettings,
				       fmt7PacketInfo.recommendedBytesPerPacket);

  if( error != PGRERROR_OK)
    { ROS_ERROR(error.GetDescription()); return false; }

  return true;
}

bool 
PgrUsbCam::configTriggerMode(bool mode = true)
{
  TriggerMode triggerMode;
  Error error = cam_.GetTriggerMode( &triggerMode);
  if(error != PGRERROR_OK)
    { ROS_ERROR(error.GetDescription()); return false; }

  triggerMode.onOff     = mode;
  triggerMode.mode      = 0;
  triggerMode.parameter = 0;

#ifdef SOFTWARE_TRIGGER_CAMERA
  // A source of 7 means software trigger
  triggerMode.source = 7;
#else
  // Trigger the camera externally using source 0.
  triggerMode.source = 0;
#endif

  error = cam_.SetTriggerMode( &triggerMode);
  if (error != PGRERROR_OK)
    { ROS_ERROR(error.GetDescription()); return false; }

  //#ifdef SOFTWARE_TRIGGER_CAMERA
  // Power on the camera
  const unsigned int k_cameraPower = 0x610;
  const unsigned int k_powerVal = 0x80000000;
  error  = cam_.WriteRegister( k_cameraPower, k_powerVal );
  if (error != PGRERROR_OK)
    { ROS_ERROR(error.GetDescription()); return false; }
  //#endif
}

void
PgrUsbCam::configImageProperty()
{
  // Set default color proc. algorithm
  Image::SetDefaultColorProcessing(HQ_LINEAR);

  // ROS image properties
  image_.encoding = sensor_msgs::image_encodings::RGB8;
  image_.height   = MAX_IMAGE_HEIGHT;
  image_.width    = MAX_IMAGE_WIDTH;
  image_.step     = MAX_IMAGE_STRIDE;
  image_.data.resize(MAX_IMAGE_SIZE);
  
  image_.is_bigendian = 0;
}

void
PgrUsbCam::setBrightness(bool _auto, unsigned int value)
{
  FlyCapture2::Property prop;
  FlyCapture2::Error error;
  prop.type = FlyCapture2::BRIGHTNESS;

#ifdef CONFIG_DEBUG
  error = cam_.GetProperty(&prop);
  if(error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());
  ROS_INFO("Brt: Auto:%u Value:%d", prop.autoManualMode, prop.valueA);  
#endif
  prop.autoManualMode = _auto;
  prop.valueA = value;

  error = cam_.SetProperty(&prop);
  if(error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());

#ifdef CONFIG_DEBUG
  error = cam_.GetProperty(&prop);
  if(error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());
  ROS_INFO("Brt: Auto:%u Value:%d <-%d", prop.autoManualMode, prop.valueA, value);  
#endif
}

void 
PgrUsbCam::setExposure(bool _auto, bool onoff, unsigned int value)
{
  FlyCapture2::Property prop;
  FlyCapture2::Error error;
  prop.type = FlyCapture2::AUTO_EXPOSURE;

#ifdef CONFIG_DEBUG
  error = cam_.GetProperty(&prop);
  if(error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());
  ROS_INFO("Exp: Auto:%u Onoff:%u Value:%d", prop.autoManualMode, prop.onOff, prop.valueA);  
#endif

  prop.autoManualMode = _auto;
  prop.onOff = onoff;
  prop.valueA = value;

  error = cam_.SetProperty(&prop);
  if(error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());

#ifdef CONFIG_DEBUG
  error = cam_.GetProperty(&prop);
  if(error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());
  ROS_INFO("Exp: Auto:%u Onoff:%u Value:%d <-%d", prop.autoManualMode, prop.onOff, prop.valueA, value);  
#endif
}

void
PgrUsbCam::setGain(bool _auto, float value)
{
  FlyCapture2::Property prop;
  FlyCapture2::Error error;
  prop.type = FlyCapture2::GAIN;

#ifdef CONFIG_DEBUG
  error = cam_.GetProperty(&prop);
  if(error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());
  ROS_INFO("Gn: Auto:%u Value:%f", prop.autoManualMode, prop.absValue);  
#endif

  prop.autoManualMode = _auto;
  prop.onOff = true;
  prop.absValue = value;
  if ((error = cam_.SetProperty(&prop)) != PGRERROR_OK)
    ROS_ERROR (error.GetDescription ());

#ifdef CONFIG_DEBUG
  error = cam_.GetProperty(&prop);
  if(error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());
  ROS_INFO("Gn: Auto:%u Value:%f", prop.autoManualMode, prop.absValue);  
#endif

  return;  
}

void
PgrUsbCam::setShutter(bool _auto, float msec)
{
  FlyCapture2::Property prop;
  FlyCapture2::Error error;
  prop.type = FlyCapture2::SHUTTER;//AUTO_EXPOSURE;

#ifdef CONFIG_DEBUG
  error = cam_.GetProperty(&prop);
  if(error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());
  ROS_INFO("Sht: Auto:%u Value:%f", prop.autoManualMode, prop.absValue);  
#endif

  prop.autoManualMode = _auto;
  prop.onOff = true;
  prop.absValue = msec;

  if ((error = cam_.SetProperty(&prop)) != PGRERROR_OK)
    ROS_ERROR (error.GetDescription ());

#ifdef CONFIG_DEBUG
  error = cam_.GetProperty(&prop);
  if(error != PGRERROR_OK)
    ROS_ERROR(error.GetDescription());
  ROS_INFO("Sht: Auto:%u Value:%f <-%f", prop.autoManualMode, prop.absValue,msec);  
#endif

  return;
}

void
PgrUsbCam::start()
{
  //cam_.StartCapture(ImageEventCallback, this);
  cam_.StartCapture();
  running_flag = true;
}

void
PgrUsbCam::stop()
{
  cam_.StopCapture();
  running_flag = false;
  cam_.Disconnect();
  connected_flag = false;
}

void
PgrUsbCam::GrabnPublish()
{
  FlyCapture2::Image buffer;
  FlyCapture2::Image convertedImage;
  cam_.RetrieveBuffer(&buffer);
  
  buffer.Convert(PIXEL_FORMAT_RGB8, &convertedImage);
  memcpy(&image_.data[0], convertedImage.GetData(), MAX_IMAGE_SIZE);
  //ROS_INFO("triggered");
  pub_.publish(image_, info_);
}

void
PgrUsbCam::ImageEventCallback(FlyCapture2::Image* pframe, const void* pData)
{
  PgrUsbCam* pSC = (PgrUsbCam*) pData;
  Image convertedImage;
#ifdef CALLBACK_DEBUG
  std::string done_msg = pSC->name_ + " frame done";
  ROS_INFO(done_msg.c_str());
#endif

#ifndef SOFTWARE_TRIGGER_CAMERA
  // @TODO: add timestamp?
  pSC->image_.header.stamp = pSC->info_.header.stamp = ros::Time::now();
#endif
  // sensor_msgs::clearImage(pSC->image_);
  // sensor_msgs::fillImage(pSC->image_, "bayer_rggb8", pframe->GetRows(),
  //		 pframe->GetCols(), pframe->GetStride(), pframe->GetData());

  pframe->Convert(PIXEL_FORMAT_RGB, &convertedImage); 
  memcpy(&pSC->image_.data[0], convertedImage.GetData(), MAX_IMAGE_SIZE);
  
  pSC->pub_.publish(pSC->image_, pSC->info_);
}

#ifdef SOFTWARE_TRIGGER_CAMERA
void
PgrUsbCam::SetTimeStamp(ros::Time t)
{
  image_.header.stamp = info_.header.stamp = t;
}

bool
PgrUsbCam::FireSoftwareTrigger()
{
#define k_softwareTrigger 0x62C
#define k_fireVal 0x80000000
  Error error = cam_.WriteRegister(k_softwareTrigger, k_fireVal);
  if (error != PGRERROR_OK)
    { ROS_ERROR(error.GetDescription()); return false; }
}
#endif
